// Copyright FYT 邹承甫 2022

#ifndef AUTOAIM_POST_PROCESSOR__PNP_SOLVER_HPP_
#define AUTOAIM_POST_PROCESSOR__PNP_SOLVER_HPP_

#include <Eigen/Core>
#include <opencv2/opencv.hpp>

namespace post_processor {

class PnPSolver {
 public:
  using Ptr = std::unique_ptr<PnPSolver>;
  PnPSolver(const std::vector<float>& camera_matrix,
            const std::vector<float>& dist_coeffs);

 public:
  Eigen::VectorXd solvePnP(std::vector<cv::Point2f>& img_pts,
                           const int& is_big);

  inline void setOpticalCenter(const cv::Point2f& optical_offset) {
    camera_matrix_.at<float>(0, 2) = optical_point_.x + optical_offset.x;
    camera_matrix_.at<float>(1, 2) = optical_point_.y + optical_offset.y;
  }

 private:
  cv::Mat camera_matrix_;
  cv::Mat dist_coeffs_;
  cv::Point2f optical_point_;
  std::vector<cv::Point3f> small_armor_pts_;
  std::vector<cv::Point3f> big_armor_pts_;
};

}  // namespace post_processor
#endif  // AUTOAIM_POST_PROCESSOR__PNP_SOLVER_HPP_